import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.DistanceSensor;

public class ProportionalPushBall extends DifferentialWheels {

	private static int S_MID_LEFT = 0; // Sensor mid left
	private static int S_FRONT_LEFT = 1; // Sensor front left
	private static int S_FRONT_RIGHT = 2; // Sensor front right
	private static int S_MID_RIGHT = 3; // Sensor mid right

	// Time to next step
	private final int TIME_STEP = 100;

	// Light Sensors
	private DistanceSensor[] _distanceSensors;

	// upper row - for left velocity - light values from 0-2
	// lower row - for right velocity - light values from 0-2
	private double[][] sensorMatrix = { { 10, 10, 0, 0 },
			{ 0, 0, 10, 10} };

	private double[] sensorValues = new double[4];
	private double[] speedValues = new double[4];

	/**
	 * Constructor
	 */
	public ProportionalPushBall() {
		super();
		initSensors();
	}

	//Get and enable the sensors
	protected void initSensors() {
		_distanceSensors = new DistanceSensor[4];
		// get lightSensor and assign that sensor to the position in the list
		_distanceSensors[0] = getDistanceSensor("ps6");
		_distanceSensors[1] = getDistanceSensor("ps7");
		_distanceSensors[2] = getDistanceSensor("ps0");
		_distanceSensors[3] = getDistanceSensor("ps1");
		for (DistanceSensor ds : _distanceSensors) {
			ds.enable(10);
		}
	}

	private void drive(double[] values) {
		setSpeed(values[0], values[1]);
	}

	/**
	 * User defined function for initializing and running the ProportionalPushBall class
	 */
	private void align() {
		while (step(TIME_STEP) != -1) {

			sensorValues[3] = _distanceSensors[S_MID_LEFT].getValue();
			sensorValues[2] = _distanceSensors[S_FRONT_LEFT].getValue();
			sensorValues[1] = _distanceSensors[S_FRONT_RIGHT].getValue();
			sensorValues[0] = _distanceSensors[S_MID_RIGHT].getValue();

			speedValues = Matrix.multiply(sensorMatrix, sensorValues);
			double[] param = new double[2];
			param[0] = Math.min(speedValues[0], 1000);
			param[1] = Math.min(speedValues[1], 1000);
			drive(param);
		}
	}

	/**
	 * Main method - in this method an instance of the controller is created and the method to launch the robot is called.
	 *
	 * @param args
	 */
	public static void main(String[] args) {
		ProportionalPushBall controller = new ProportionalPushBall();
		// Search the light as first state
		controller.align();
	}
}
